/*
Laboratorio 1

Tartaro Deniz	Rizzi Alberto	Longo Filippo
1013067 	1013068		1013297 

Aggirare un ostacolo
*/


#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/TwistWithCovariance.h>
#include <nxt_msgs/Range.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <stdio.h>

ros::Publisher vel_pub;
geometry_msgs::Twist velocity;

double range, pi=3.14;
double lStep=0.002, aStep=0.03;
double lMax=0.15, aMax=2.0;
double lVel=0, aVel=0;
float y=0, z=0;
double angolo=0;


//callback per il range
void callbackRange(const nxt_msgs::Range::ConstPtr& msg) {
	
	range = msg->range;		
}

//callback per il range
void callbackOdom(const nav_msgs::Odometry::ConstPtr& msg) {
	
	y=msg->pose.pose.position.y;
	z=msg->pose.pose.position.z;
	geometry_msgs::Quaternion msg_q=msg->pose.pose.orientation;
	angolo = tf::getYaw(msg_q);
}



// aggiustamenti per mantenersi lungo il verso dell'angolo targetAngle
void stayStraight(double targetAngle) {
	
	if(angolo>targetAngle){
		
		velocity.angular.z = -0.06;
	}
	
	if(angolo<targetAngle){
		
		velocity.angular.z = 0.06;
	}
}

// va dritto finche non incontra un ostacolo a distanza range <= 0.30
void goStraight(double direction, int loopRate) {
	
	ros::Rate loop_rate(loopRate);
	
	range=10;
	lVel=0;
	
	while(range>0.30){
				
		velocity.angular.z = 0.00;
		
		if(lVel < lMax)
			lVel+=lStep;
		velocity.linear.x = lVel;
		
		stayStraight(direction);
		
		vel_pub.publish(velocity);
		ros::spinOnce();
		loop_rate.sleep();
		
	}
	
	velocity.linear.x = 0;
	vel_pub.publish(velocity);
	
	return;
}

// va dritto per timeInterval secondi, nella direzione direction
void goStraight(double timeInterval, double direction, int loopRate) {
	
	ros::Rate loop_rate(loopRate);
	
	double start = ros::Time::now().toSec();
	lVel=0;
	
	while((ros::Time::now().toSec()- start) < timeInterval){
		
		velocity.angular.z = 0.0;
		
		if(lVel<lMax)
			lVel+=lStep;
		velocity.linear.x = lVel;
	
		stayStraight(direction);
	
		vel_pub.publish(velocity);
		ros::spinOnce();
		loop_rate.sleep();
	}
	
	velocity.linear.x = 0;
	vel_pub.publish(velocity);
	
	return;
}


//va usato dentro un while(CONDIZIONE_RELATIVA_ALL'ANGOLO_IN_CUI_SI_DEVE_FERMARE)
void turnLeft(int rate) {
	ros::Rate loop_rate(rate);

	if(aVel<aMax)
		aVel+=aStep;
	velocity.angular.z = aVel;
	velocity.linear.x = 0.00;
	vel_pub.publish(velocity);
	ros::spinOnce();
	loop_rate.sleep();
	
	return;
}

//va usato dentro un while(CONDIZIONE_RELATIVA_ALL'ANGOLO_IN_CUI_SI_DEVE_FERMARE)
void turnRight(int rate) {
	ros::Rate loop_rate(rate);
	if(aVel>-aMax)
		aVel-=aStep;
	velocity.angular.z = aVel;
	velocity.linear.x = 0.00;
	vel_pub.publish(velocity);
	ros::spinOnce();
	loop_rate.sleep();
	
	return;
}

//metodo che effettua l'aggiramento dell'ostacolo
void avoidObstacle(bool isReturning, int loopRate) {

	ros::Rate loop_rate(loopRate);
	double ob1,ob2,direction, del;

	lVel=0;
	aVel=0;

	del = 0.395; //delta di aggiustamento dell'angolo

	if(!isReturning){

		direction = 0;
		ob1 = -(pi/2) + del;
		ob2 =  (pi/2) - del;

	}
	else{
				
		direction = -2.5;
		ob1 = 2.2 + del;
		ob2 = direction + (pi/2) - del;

	}

	//destra 90
	while( (angolo>ob1) || (angolo<-2.0) ){
			
		turnRight(loopRate);
				
	}
	
	//dritto per 3.5 secondi
	goStraight(3.5, angolo, loopRate);
	
	aVel=0;
	
	if(isReturning)
		direction=-2.6; //correzzione dovuta ad errori
	else
		direction-=0.1; //correzzione dovuta ad errori

	//sinistra 90
	//condizione1: angolo<direction   # condizione2: angolo>0 || angolo<direction
	while( (angolo>1.5) || (angolo<direction) ){
		
		turnLeft(loopRate);
		
	}
	
	//dritto per 5.5 secondi
	goStraight(5.5, angolo, loopRate);
	
	aVel=0;
	
	ob2-=0.17; //correzzione dovuta ad errori

	//sinistra 90
	while(angolo<ob2){
		
		turnLeft(loopRate);
		
	}
	
	//dritto per 3.5 secondi
	goStraight(3.5, angolo, loopRate);
	
	aVel=0;
	
	if(isReturning)
		direction-=0.10; //correzzione dovuta ad errori
	
	//destra 90 
	while(angolo>direction+0.20){
		
		turnRight(loopRate);
		
	}
	
	lVel = 0;
	aVel=0;

	return;
}

//inversione di 180 gradi
void turnBack(int loopRate) {

	ros::Rate loop_rate(loopRate);
	double direction=pi-0.74;

	double aVel=0;

	while((angolo<0 && angolo>-direction)|| (angolo>0 && angolo<direction/2)){
		
		turnRight(loopRate);
		
	}
	
	return;

}


int main(int argc, char **argv) {

	ros::init(argc, argv, "lab1");

	ros::Subscriber r_sub, odom_sub;
	ros::NodeHandle ns, np;
	
	vel_pub = np.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
	r_sub = ns.subscribe("ultrasonic_sensor", 1000, callbackRange);
	odom_sub = ns.subscribe("odom", 1000, callbackOdom);

	bool isReturning = false;
	double startingDirection = 0;
	double returningDirection = -2.6;
	
	velocity.angular.x = 0.00;
	velocity.angular.y = 0.00;
	velocity.angular.z = 0.00;
	velocity.linear.x = 0.00;
	velocity.linear.y = 0.00;
	velocity.linear.z = 0.00;
	vel_pub.publish(velocity);
	
	
	
	double t1=ros::Time::now().toSec();
					
	goStraight(startingDirection, 15);
	
	double t2=(ros::Time::now().toSec()-1);
		
	avoidObstacle(isReturning, 50);
	
	startingDirection-=0.05; //correzzione dovuta ad errori

	goStraight(startingDirection, 30);
	
	// Aspetta 10 secondi
	double tw=ros::Time::now().toSec();
	while(ros::Time::now().toSec()-tw <10)
	{
		velocity.linear.x = 0.0;
		velocity.angular.z = 0.0;
		vel_pub.publish(velocity);
	}	
	
	turnBack(50);
			
	isReturning = true; //inizia la parte di ritorno

	goStraight(returningDirection, 30);
			
	avoidObstacle(isReturning, 50);
		
	returningDirection=-2.50; //correzzione dovuta ad errori
	
	goStraight(t2-t1, returningDirection, 15);
		

	//stop robot
	velocity.linear.x = 0.0;
	velocity.angular.z = 0.0;
	vel_pub.publish(velocity);

	ros::shutdown();

	return 0;
}

